/**
 * @mainpage
 *
 * @author  Ryan T. Sammartino <ryan.sammartino@swordfishstudios.com>
 * @date    Tue Sep 18 16:23:41 2007
 * @version 1.5
 * 
 * @brief  Replacements for trigonometric functions for PS3.
 *
 * This library contains fast replacements for:
 *   - @c sin
 *   - @c cos
 *   - @c tan
 *   - @c sinf
 *   - @c cosf
 *   - @c tanf
 *   - @c _FSin
 *   - @c _Sin
 *   - @c asin
 *   - @c acos
 *   - @c asinf
 *   - @c acosf
 *
 * as well as an optimised @c sincos function.
 *
 * @attention
 *   - The @c tan function returns values that are significantly
 *     different than the standard libraries when you approach
 *     +/- pi/2 (and multiples thereof); this is to be expected, given
 *     the discontinuity there.
 *
 * @section timings Timings
 *
 * Obtained from one run of the test suite (40,000 calls to each
 * function):
 *
 * <pre>
 *    __builtin_sin    : 1600274 cycles
 *    ps3_sin          : 187696 cycles
 *    Speed up factor  : 8.53
 *
 *    __builtin_cos    : 1597595 cycles
 *    ps3_cos          : 194281 cycles
 *    Speed up factor  : 8.22
 *
 *    __builtin_sincos : 3165353 cycles
 *    ps3_sincos       : 224685 cycles
 *    Speed up factor  : 14.1
 *
 *    __builtin_tan    : 1620564 cycles
 *    ps3_tan          : 282826 cycles
 *    Speed up factor  : 5.73
 *
 *    __builtin_asin   : 1179231 cycles
 *    ps3_asin         : 333364 cycles
 *    Speed up factor  : 3.54
 *
 *    __builtin_acos   : 1145103 cycles
 *    ps3_acos         : 311042 cycles
 *    Speed up factor  : 3.68
 * </pre>
 *
 * @section sym_replace Symbol Replacement
 *
 * If you compile with
 *
 * <tt>-D_TRIG_REPLACE</tt>
 *
 * and if you link with
 *
 * <tt>-Wl,--wrap=sin  -Wl,--wrap=cos  -Wl,--wrap=tan
 *     -Wl,--wrap=sinf -Wl,--wrap=cosf -Wl,--wrap=tanf
 *     -Wl,--wrap=_FSin -Wl,--wrap=_Sin -Wl,--wrap=asin
 *     -Wl,--wrap=asinf -Wl,--wrap=acos -Wl,--wrap=acosf</tt>
 *
 * then all calls to those functions, including calls in libraries
 * that you may not have the source code for, will be routed through
 * these functions.
 *
 * @section copyright Copyright and License
 *
 * Copyright (c) 2007 Vivendi Games.
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation files
 * (the "Software"), to deal in the Software without restriction,
 * including without limitation the rights to use, copy, modify, merge,
 * publish, distribute, sublicense, and/or sell copies of the Software,
 * and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

/*****************************************************************/

// Constants.

#ifndef __SNC__

static const double PI      = 3.14159265358979323846;
static const double HALF_PI = PI / 2.0;
static const double PI_4    = PI / 4.0;
static const double INV_PI  = 1.0 / PI;
static const double ZERO    = 0.0;
static const double ONE     = 1.0;
static const double NEG_ONE = -1.0;
static const double HALF    = 0.5;
static const double TWO     = 2.0;
/** sin Taylor series coefficients. */
static const double SC[]     =
{
    -1.6666666666666666666667e-01, // 1/3!
    +8.3333333333333333333333e-03, // 1/5!
    -1.9841269841269841269841e-04, // 1/7!
    +2.7557319223985890652557e-06, // 1/9!
    -2.5052108385441718775052e-08, // 1/11!
    +1.6059043836821614599392e-10, // 1/13!
    -7.6471637318198164759011e-13, // 1/15!
    +2.8114572543455207631989e-15, // 1/17!
};
/** cos Taylor series coefficients. */
static const double CC[]    =
{
    -5.0000000000000000000000e-01, // 1/2!
    +4.1666666666666666666667e-02, // 1/4!
    -1.3888888888888888888889e-03, // 1/6!
    +2.4801587301587301587302e-05, // 1/8!
    -2.7557319223985890652557e-07, // 1/10!
    +2.0876756987868098979210e-09, // 1/12!
    -1.1470745597729724713852e-11, // 1/14!
    +4.7794773323873852974382e-14, // 1/16!   
};
/** asin/acos numerator coefficients. */
static const double PC[] =
{
    -2.7368494524164255994e+01,
    +5.7208227877891731407e+01,
    -3.9688862997404877339e+01,
    +1.0152522233806463645e+01,
    -6.9674573447350646411e-01,
};
/** asin/acos denominator coefficients. */
static const double QC[] =
{
    -1.6421096714498560795e+02,
    +4.1714430248260412556e+02,
    -3.8186303361750149284e+02,
    +1.5095270841030604719e+02,
    -2.3823859153670238830e+01, 
};
static const double VERY_SMALL_NUMBER = 1e-20;

/*****************************************************************/

// My own intrinsics.  ppu_intrinsics.h doesn't always do
// what I want (also, it is different from Sony's version
// to IBM's Cell Simulator version, which is what I use
// for development).

static double __fctid(double) __attribute__((always_inline));
/** 
 * Rounds @a x to integer.
 * 
 * @param x Number to convert.
 * 
 * @return @a x as an integer.
 */
static double __fctid(double x)
{
    double r;
    __asm__ ("fctid %0, %1"
             : "=f"(r) : "f"(x));
    return r;
}

static double __fctidz(double) __attribute__((always_inline));
/** 
 * Rounds @a x to integer (towards zero).
 * 
 * @param x Number to convert.
 * 
 * @return @a x rounded towards zero.
 */
static double __fctidz(double x)
{
    double r;
    __asm__ ("fctidz %0, %1"
             : "=f"(r) : "f"(x));
    return r;
}

static double __fcfid(double) __attribute__((always_inline));
/** 
 * Converts @a x to double.
 * 
 * @param x Number to convert.
 * 
 * @return @a x as a double.
 */
static double __fcfid(double x)
{
    double r;
    __asm__ ("fcfid %0, %1"
             : "=f"(r) : "f"(x));
    return r;
}

static double __fabs(double) __attribute__((always_inline));
/** 
 * Computes |@a x|.
 * 
 * @param x Number to take absolute value of.
 * 
 * @return |@a x|.
 */
static double __fabs(double x)
{
    double r;
    __asm__ ("fabs %0, %1"
             : "=f"(r) : "f"(x));
    return r;
}

static double __fsqrt(double) __attribute__((always_inline));
/** 
 * Computes the square root of @a x.
 * 
 * @param x Number to take square root of
 * 
 * @return Square root of @a x.
 */
static double __fsqrt(double x)
{
    double r;
    __asm__ ("fsqrt %0, %1"
             : "=f"(r) : "f"(x));
    return r;
}

static double __fsel(double, double, double) __attribute__((always_inline));
/** 
 * Does a branchless selection.
 * 
 * @param c Comparand.
 * @param gte Value to return if @a c >= 0.
 * @param lt Value to return if @a c < 0.
 * 
 * @return <tt>c >= 0 ? gte : lt</tt>.
 */
static double __fsel(double c, double gte, double lt)
{
    double r;
    __asm__ ("fsel %0, %1, %2, %3"
             : "=f"(r) : "f"(c), "f"(gte), "f"(lt));
    return r;
}

static double __round(double) __attribute__((always_inline));
/** 
 * Rounds @a x to nearest integer.
 * 
 * @param x Number to round.
 * 
 * @return @a x rounded to nearest integer.
 */
static double __round(double x)
{
    return __fcfid(__fctid(x));
}

static double __trunc(double) __attribute__((always_inline));
/** 
 * Truncates @a x (round to zero).
 * 
 * @param x Number to truncate.
 * 
 * @return @a x truncated (rounded to zero).
 */
static double __trunc(double x)
{
    return __fcfid(__fctidz(x));
}

/*****************************************************************/

// Functions.

double ps3_sin(double) __attribute__((always_inline));
/** 
 * Computes the sine of @a x.
 * 
 * @param x angle (radians) 
 * 
 * @return sin(@a x)
 */
double ps3_sin(double x)
{
    double absx = __fabs(x);
    
    double sign = __fsel(x, ONE, NEG_ONE);

    // Normalise x
    double num_cycles = __round(absx * INV_PI);

    // Determine if num_cycles is even or odd
    double is_even = __trunc(num_cycles * HALF) - (num_cycles * HALF);

    // -pi/2 <= norm_x < pi/2
    double norm_x = absx - PI * num_cycles;

    // Run Taylor series with funky factorisation.
    double y = norm_x * norm_x;

    double taylor = ((((((((SC[7] * y + SC[6])
                           * y + SC[5])
                          * y + SC[4])
                         * y + SC[3])
                        * y + SC[2])
                       * y + SC[1])
                      * y + SC[0])
                     * y);

    sign = __fsel(is_even, sign, -sign);

    double result = norm_x + norm_x * taylor;

    result *= sign;

    return result;
}

double ps3_cos(double) __attribute__((always_inline));
/** 
 * Computes the cosine of @a x.
 * 
 * @param x angle (radians)
 * 
 * @return cos(@a x)
 */
double ps3_cos(double x)
{
    return ps3_sin(x + HALF_PI);
}

void ps3_sincos(double, double *, double *) __attribute__((always_inline));
/** 
 * Computes the sine and cosine of @a x.
 * @a s and @a c may not be @c NULL.
 * 
 * @param[in]  x angle (radians)
 * @param[out] s sin(@a x) 
 * @param[out] c cos(@a x)
 */
void ps3_sincos(double x, double *s, double *c)
{
    double absx = __fabs(x);
    // "Cosine" |x|.
    double cabsx = absx + HALF_PI;
    
    double sign = __fsel(x, ONE, NEG_ONE);

    double num_cycles  = __round(absx * INV_PI);
    
    // Determine if num_cycles is even or odd.
    double is_even = __trunc(num_cycles * HALF) - (num_cycles * HALF);

    // Normalise.
    double norm_x  = absx - PI * num_cycles;
    double norm_cx = cabsx - PI * num_cycles - HALF_PI;

    // Run Taylor series with funky factorisation.
    double y  = norm_x * norm_x;
    double cy = norm_cx * norm_cx;

    double taylor = ((((((((SC[7] * y + SC[6])
                           * y + SC[5])
                          * y + SC[4])
                         * y + SC[3])
                        * y + SC[2])
                       * y + SC[1])
                      * y + SC[0])
                     * y);
    double taylorc = ((((((((CC[7] * cy + CC[6])
                            * cy + CC[5])
                           * cy + CC[4])
                          * cy + CC[3])
                         * cy + CC[2])
                        * cy + CC[1])
                       * cy + CC[0])
                      * cy);

    sign = __fsel(is_even, sign, -sign);
    double cos_sign = __fsel(is_even, ONE, NEG_ONE);

    double sine = norm_x + norm_x * taylor;
    double cosine = ONE + taylorc;

    sine *= sign;
    cosine *= cos_sign;

    *s = sine;
    *c = cosine;
}

/** 
 * Computes the sine and cosine of @a fx.
 * @a fs and @a fc may not be @c NULL.
 * 
 * @param[in]  fx angle (radians)
 * @param[out] fs sin(@a fx)
 * @param[out] fc cos(@a fx)
 */
void ps3_sincos(float fx, float *fs, float *fc)
{
    double x = static_cast<double>(fx);
    double s, c;
    ps3_sincos(x, &s, &c);
    *fs = static_cast<float>(s);
    *fc = static_cast<float>(c);
}

double ps3_tan(double) __attribute__((always_inline));
/** 
 * Computes the tangent of @a x.
 * 
 * @param x angle (radians)
 * 
 * @return tan(@a x)
 */
double ps3_tan(double x)
{
    double s, c;
    ps3_sincos(x, &s, &c);

    // Ensure c isn't zero.
    double absc = __fabs(c);
    
    double is_cos_not_zero = absc - VERY_SMALL_NUMBER;
    c = __fsel(is_cos_not_zero, c, VERY_SMALL_NUMBER);
    
    return s/c;
}

double ps3_asin(double) __attribute__((always_inline));
/** 
 * Computes the inverse sine of @a x.
 * 
 * @param x ratio (-1.0 <= @a x <= 1.0)
 * 
 * @return @a y such that @a x = sin(@a y)
 */
double ps3_asin(double x)
{
    // A branchless implementation of the algorithm found
    // in "Software Manual for the Elementary Functions".
    double abs_x = __fabs(x);

    double g = __fsel(abs_x - HALF, (ONE - abs_x) * HALF, abs_x * abs_x);
    double y = __fsel(abs_x - HALF, -TWO * __fsqrt(g), abs_x);

    double P = (((((PC[4] * g + PC[3])
                   * g + PC[2])
                  * g + PC[1])
                 * g + PC[0])
                * g);
    double Q = (((((g + QC[4])
                   * g + QC[3])
                  * g + QC[2])
                 * g + QC[1])
                * g + QC[0]);
    double R = y + y * P / Q;

    double res = __fsel(abs_x - HALF, HALF_PI + R, R);

    return __fsel(x, res, -res);
}

double ps3_acos(double) __attribute__((always_inline));
/** 
 * Computes the inverse cosine of @a x.
 * 
 * @param x ratio (-1.0 <= @a x <= 1.0)
 * 
 * @return @a y such that @a x = cos(@a y)
 */
double ps3_acos(double x)
{
    // A branchless implementation of the algorithm found
    // in "Software Manual for the Elementary Functions".
    double abs_x = __fabs(x);

    double g = __fsel(abs_x - HALF, (ONE - abs_x) * HALF, abs_x * abs_x);
    double y = __fsel(abs_x - HALF, -2.0 * __fsqrt(g), abs_x);
    double b = __fsel(abs_x - HALF, HALF_PI, PI_4);
    double a = __fsel(abs_x - HALF, ZERO, PI_4);

    double P = (((((PC[4] * g + PC[3])
                   * g + PC[2])
                  * g + PC[1])
                 * g + PC[0])
                * g);
    double Q = (((((g + QC[4])
                   * g + QC[3])
                  * g + QC[2])
                 * g + QC[1])
                * g + QC[0]);
    double R = y + y * P / Q;

    return __fsel(x, TWO * a - R, TWO * b + R);
}

/*****************************************************************/

// Wrappers for linker symbol replacement.

double __wrap_sin(double) asm("__wrap_sin");
/** 
 * A wrapper function for symbol replacement.
 * To replace all occurrences of the standard library's @c sin
 * function, link with:
 *
 * <tt>-Wl,--wrap=sin</tt>
 * 
 * @param x angle (radians)
 * 
 * @return sin(@a x)
 */
double __wrap_sin(double x)
{
    return ps3_sin(x);
}

double __wrap_cos(double) asm("__wrap_cos");
/** 
 * A wrapper function for symbol replacement.
 * To replace all occurrences of the standard library's @c cos
 * function, link with:
 *
 * <tt>-Wl,--wrap=cos</tt>
 * 
 * @param x angle (radians)
 * 
 * @return cos(@a x)
 */
double __wrap_cos(double x)
{
    return ps3_cos(x);
}

double __wrap_tan(double) asm("__wrap_tan");
/** 
 * A wrapper function for symbol replacement.
 * To replace all occurrences of the standard library's @c tan
 * function, link with:
 *
 * <tt>-Wl,--wrap=tan</tt>
 * 
 * @param x angle (radians)
 * 
 * @return tan(@a x)
 */
double __wrap_tan(double x)
{
    return ps3_tan(x);
}


double __wrap_asin(double) asm("__wrap_asin");
/** 
 * A wrapper function for symbol replacement.
 * To replace all occurrences of the standard library's @c asin
 * function, link with:
 *
 * <tt>-Wl,--wrap=asin</tt>
 * 
 * @param x ratio (-1.0 <= @a x <= 1.0)
 * 
 * @return @a y such that @a x = sin(@a y)
 */
double __wrap_asin(double x)
{
    return ps3_asin(x);
}

double __wrap_acos(double) asm("__wrap_acos");
/** 
 * A wrapper function for symbol replacement.
 * To replace all occurrences of the standard library's @c acos
 * function, link with:
 *
 * <tt>-Wl,--wrap=acos</tt>
 * 
 * @param x ratio (-1.0 <= @a x <= 1.0)
 * 
 * @return @a y such that @a x = cos(@a y)
 */
double __wrap_acos(double x)
{
    return ps3_acos(x);
}


float __wrap_sinf(float) asm("__wrap_sinf");
/** 
 * A wrapper function for symbol replacement.
 * To replace all occurrences of the standard library's @c sinf
 * function, link with:
 *
 * <tt>-Wl,--wrap=sinf</tt>
 * 
 * @param x angle (radians)
 * 
 * @return sinf(@a x)
 */
float __wrap_sinf(float x)
{
    return static_cast<float>(ps3_sin(static_cast<double>(x)));
}

float __wrap_cosf(float) asm("__wrap_cosf");
/** 
 * A wrapper function for symbol replacement.
 * To replace all occurrences of the standard library's @c cosf
 * function, link with:
 *
 * <tt>-Wl,--wrap=cosf</tt>
 * 
 * @param x angle (radians)
 * 
 * @return cosf(@a x)
 */
float __wrap_cosf(float x)
{
    return static_cast<float>(ps3_cos(static_cast<double>(x)));
}

float __wrap_tanf(float) asm("__wrap_tanf");
/** 
 * A wrapper function for symbol replacement.
 * To replace all occurrences of the standard library's @c tanf
 * function, link with:
 *
 * <tt>-Wl,--wrap=tanf</tt>
 * 
 * @param x angle (radians)
 * 
 * @return tanf(@a x)
 */
float __wrap_tanf(float x)
{
    return static_cast<float>(ps3_tan(static_cast<double>(x)));
}


float __wrap_asinf(float) asm("__wrap_asinf");
/** 
 * A wrapper function for symbol replacement.
 * To replace all occurrences of the standard library's @c asinf
 * function, link with:
 *
 * <tt>-Wl,--wrap=asinf</tt>
 * 
 * @param x ratio (-1.0 <= @a x <= 1.0)
 * 
 * @return @a y such that @a x = sin(@a y)
 */
float __wrap_asinf(float x)
{
    return static_cast<float>(ps3_asin(static_cast<double>(x)));
}

float __wrap_acosf(float) asm("__wrap_acosf");
/** 
 * A wrapper function for symbol replacement.
 * To replace all occurrences of the standard library's @c acosf
 * function, link with:
 *
 * <tt>-Wl,--wrap=acosf</tt>
 * 
 * @param x ratio (-1.0 <= @a x <= 1.0)
 * 
 * @return @a y such that @a x = cos(@a y)
 */
float __wrap_acosf(float x)
{
    return static_cast<float>(ps3_acos(static_cast<double>(x)));
}


/** A reference to the "real" @c _FSin function. */
extern float __real__FSin(float, unsigned int) asm("__real__FSin");

float __wrap__FSin(float, unsigned int) asm("__wrap__FSin");
/** 
 * A wrapper function for symbol replacement.
 * To replace all occurrences of Sony's @c _FSin function, compile with:
 *
 * <tt>-D_TRIG_REPLACE</tt>
 *
 * and link with:
 *
 * <tt>-Wl,--wrap=_FSin</tt>
 * 
 * @param x angle (radians)
 * @param op operation: 0 == sin, 1 == cos
 * 
 * @return sin(@a x) iff @a op == 0, cos(@a x) iff @a op == 1, otherwise
 *         pass through to Sony's @c _FSin function.
 */
float __wrap__FSin(float x, unsigned int op)
{
    if (op == 0)
    {
        return __wrap_sinf(x);
    }
    else if (op == 1)
    {
        return __wrap_cosf(x);
    }
    else
    {
        // Unknown op: pass through to "real" _FSin function.
        return __real__FSin(x, op);
    }
}

/** A reference to the "real" @c _Sin function. */
extern double __real__Sin(double, unsigned int) asm("__real__Sin");

double __wrap__Sin(double, unsigned int) asm("__wrap__Sin");
/** 
 * A wrapper function for symbol replacement.
 * To replace all occurrences of Sony's @c _Sin function, compile with:
 *
 * <tt>-D_TRIG_REPLACE</tt>
 *
 * and link with:
 *
 * <tt>-Wl,--wrap=_Sin</tt>
 * 
 * @param x angle (radians)
 * @param op operation: 0 == sin, 1 == cos
 * 
 * @return sin(@a x) iff @a op == 0, cos(@a x) iff @a op == 1, otherwise
 *         pass through to Sony's @c _Sin function.
 */
double __wrap__Sin(double x, unsigned int op)
{
    if (op == 0)
    {
        return __wrap_sin(x);
    }
    else if (op == 1)
    {
        return __wrap_cos(x);
    }
    else
    {
        // Unknown op: pass through to "real" _FSin function.
        return __real__Sin(x, op);
    }
}

#endif